#include "SceneGraphNode.h"

namespace scenegraph
{

	SceneGraphNode::SceneGraphNode() {
		int row, col;
		// Initialize transformation matrix
		for ( row=0; row<4; row++ ) {
			for ( col=0; col<4; col++ ) {
				if ( row==col ) {
					transform[4*col+row] = 1.0f;
				} else {
					transform[4*col+row] = 0.0f;
				}
			}
		}
		rotAxis[0] = 0.0f; 
		rotAxis[1] = 0.0f; 
		rotAxis[2] = 1.0f; 
		angle = 0.0f;
		translate[0] = 0.0f; 
		translate[1] = 0.0f; 
		translate[2] = 0.0f;
		scale[0] = 1.0f; 
		scale[1] = 1.0f; 
		scale[2] = 1.0f;
	}

	SceneGraphNode::~SceneGraphNode() 
	{
		// Allow subclass destructors to be called
	}

	void SceneGraphNode::destroy() 
	{
		// nothing to be done
	}

	Vector3D SceneGraphNode::getTranslation()
	{
		return Vector3D(translate[0], translate[1], translate[2]);
	}

	//setTranslation methods
	void SceneGraphNode::setTranslation( const float _translate[3] ) 
	{
		for ( int col=0; col<3; col++ ) 
		{
			translate[col] = _translate[col];
		}
		updateTransform();
	}
	void SceneGraphNode::setTranslation(float x, float y, float z) 
	{
		translate[0] = x;
		translate[1] = y;
		translate[2] = z;
		updateTransform();
	}
	void SceneGraphNode::setTranslation(const Vector3D translation)
	{
		translate[0] = translation.X;
		translate[1] = translation.Y;
		translate[2] = translation.Z;
		updateTransform();
	}

	//incTranslation methods
	void SceneGraphNode::incTranslation( const float _translate[3] ) 
	{
		for ( int col=0; col<3; col++ ) {
		translate[col] += _translate[col];
		}
		updateTransform();
	}
	void SceneGraphNode::incTranslation(float x, float y, float z) {
		translate[0] += x;
		translate[1] += y;
		translate[2] += z;
		updateTransform();
	}
	void SceneGraphNode::incTranslation(const Vector3D translation)
	{
		translate[0] += translation.X;
		translate[1] += translation.Y;
		translate[2] += translation.Z;
		updateTransform();
	}

	//operator overloading
	void SceneGraphNode::operator +=(const collisions::Vector3D translate)
	{
		incTranslation(translate);
	}
	void SceneGraphNode::operator -=(const collisions::Vector3D translate)
	{
		incTranslation(-translate);
	}
	 
	//setScale methods
	void SceneGraphNode::setScale( const float _scale[3] ) 
	{
		for ( int col=0; col<3; col++ ) 
		{
			scale[col] = _scale[col];
		}
		updateTransform();
	}
	void SceneGraphNode::setScale(float x, float y, float z) 
	{
		scale[0] = x;
		scale[1] = y;
		scale[2] = z;
		updateTransform();
	}
	void SceneGraphNode::setScale(const Vector3D newScale)
	{
		scale[0] = newScale.X;
		scale[1] = newScale.Y;
		scale[2] = newScale.Z;
		updateTransform();
	}

	//incScale methods
	void SceneGraphNode::incScale( const float _scale[3] ) {
		for ( int col=0; col<3; col++ ) 
		{
			scale[col] += _scale[col];
		}
		updateTransform();
	}
	void SceneGraphNode::incScale(float x, float y, float z) 
	{
		scale[0] += x;
		scale[1] += y;
		scale[2] += z;
		updateTransform();
	}
	void SceneGraphNode::incScale(const Vector3D newScale)
	{
		scale[0] += newScale.X;
		scale[1] += newScale.Y;
		scale[2] += newScale.Z;
		updateTransform();
	}


	//rotation methods
	void SceneGraphNode::setAxisAngle( const float _axis[3], float _angle ) 
	{
		for ( int col=0; col<3; col++ ) 
		{
			rotAxis[col] = _axis[col];
		}
		angle = _angle;
		updateTransform();
	}
	void SceneGraphNode::setAxisAngle(float x, float y, float z, float _angle)
	{
		rotAxis[0] = x;
		rotAxis[1] = y;
		rotAxis[2] = z;
		angle = _angle;
		updateTransform();
	}
	void SceneGraphNode::setAxisAngle(const collisions::Vector3D axis, float _angle)
	{
		rotAxis[0] = axis.X;
		rotAxis[1] = axis.Y;
		rotAxis[2] = axis.Z;
		angle = _angle;
		updateTransform();
	}

	//retrieves current rotation info 
	void SceneGraphNode::getAxisAngle( float _axis[3], float* _angle ) 
	{
		for ( int col=0; col<3; col++ ) 
		{
			_axis[col] = rotAxis[col];
		}
		*_angle = angle;
	}
	void SceneGraphNode::getAxisAngle(collisions::Vector3D *axis, float *_angle)
	{
		axis->X = rotAxis[0];
		axis->Y = rotAxis[1];
		axis->Z = rotAxis[2];
		*_angle = angle;
	}
	void SceneGraphNode::getAxisAngle(float *x, float *y, float *z, float *_angle)
	{
		*x = rotAxis[0];
		*y = rotAxis[1];
		*z = rotAxis[2];
		*_angle = angle;
	}

	void SceneGraphNode::updateTransform() {
		glMatrixMode( GL_MODELVIEW );
		glPushMatrix();
		glLoadIdentity();
		glTranslatef( translate[0], translate[1], translate[2] );
		glRotatef( angle, rotAxis[0], rotAxis[1], rotAxis[2] );
		glScalef( scale[0], scale[1], scale[2] );
		// Read the rotation matrix back
		glGetFloatv(GL_MODELVIEW_MATRIX, transform );
		glPopMatrix();
	}
}